#define P_LED 13
#define SERVO_STEERING 11
#define SERVO_MOVEMENT 10
#define CENTER_RANGE 5

/*
 Rutinas de control de CiclopEee
*/

// Recibe un \u00e1ngulo de giro, valores desde -45 a 45 grados
#include "WProgram.h"
void steer(int angle);
void throttle(int power);
void blink(int ms);
void blink(int ms, int repeats);
void setup();
void loop();
void steer(int angle)
{
  analogWrite(SERVO_STEERING,map(-angle,-45,45,100,240)); 
}

// Maneja el avance o retroceso, recibe la velocidad desde -100 a 100 porciento
void throttle(int power)
{
  analogWrite(SERVO_MOVEMENT,(power>CENTER_RANGE || power<-CENTER_RANGE) ? map(-power,-100,100,135,225):0); 
}

void blink(int ms)
{
  digitalWrite(P_LED,HIGH);
  delay(ms);
  digitalWrite(P_LED,LOW);
}

void blink(int ms, int repeats)
{
  for(int i=0;i<repeats;i++)
  {
    blink(ms/2);
    delay(ms/2);
  }
}

void setup()
{
  pinMode(P_LED,OUTPUT);
  steer(0);
  throttle(0);
  Serial.begin(9600);
  blink(100,10);
}

void loop()
{
  if(Serial.available()>=3)
    if(Serial.read()==0xEE)
    {
      int val_s = Serial.read(), val_t = Serial.read();;
      
      if(val_s!=0xFF)
        steer(map(val_s,0,90,-45,45));
        
      if(val_t!=0xFF)
        throttle(map(val_t,0,200,-100,100));
    }    
}
int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

